/**
 * @file demo.c
 * @author SunWater
 * @brief 这是一个演示文件
 * 用于演示如何使用蜘蛛腿部算法
 * @version 0.1
 * @date 2025-04-06
 *
 */

#include <demo.h>

#include "0A_robot_devices.h"
#include "arm_math.h"
#include "cmsis_os.h"
#include "math.h"
#include "servo.h"
#include "spider_leg.h"

// ----------------常量定义------------------
#define SERVO_1_CENTER 135.0f
#define SERVO_2_CENTER 135.0f
#define SERVO_3_CENTER 135.0f

//--------------本地函数声明------	------------
// 获得运动轨迹的目标位置
static void get_pos_by_path(float x, float y, float z, float radinus,
                            float current_time, float loop_time, float *output);

// 运动函数
static void walk_forward_tripedal(void);
static void walk_forward_five_leg(void);

// 胶水推杆控制函数
static void glue_pusher(_Bool pusher_on);

// --------------DEBUG变量------------------
float deg = 0;                        // 舵机角度
float goal_pos[3] = {0};              // 目标位置
float servo_rad[3] = {0};             // 舵机角度
Leg_IK_Status ik_status = LEG_IK_OK;  // 逆运动学状态

RobotControl robot_control = {
    // 是否开始按轨迹运动
    .start_loop = 0,

    // 步态选择
    .foot_style = FIVE_LEG,

    // 步态调整参数
    .yaw_change = -15,
    .yaw_fix = 5,
    .FL_pitch_change_J2 = 45,
    .FL_pitch_change_J3 = 20,

    // 气泵和胶水开关
    .pump_all_on = 0,
    .glue = 0,
};

// 这个等待时间需要根据舵机的旋转速度调整
// 最好每次计算得出下一个角度前，舵机已转到上一个角度
// 以免舵机速度更不上计算速度，导致舵机转动不平滑
uint16_t WAIT_TIME = 1;  // 等待时间，单位ms

//-------------主函数实现------------------
// 以下函数以多线程运行，但我没有配置，这里仅做演示
void demo_task(void) {
    // 设置腿的初始位置，连接腿
    Leg_set_pos(&leg_1, &leg_2, &leg_3, leg_1.start_angle_yaw,
                leg_2.start_angle_pitch, leg_3.start_angle_pitch, 1);

    // 获得运动轨迹的圆心
    float center_x = leg_3.end_x;  // 圆心位置x
    float center_y = leg_3.end_y;  // 圆心位置y
    float center_z = leg_3.end_z;  // 圆心位置z

    // 舵机运动到初始位置
    LG_spin_to_deg(&leg_group_1, SERVO_1_CENTER, SERVO_2_CENTER,
                   SERVO_3_CENTER);
    LG_spin_to_deg(&leg_group_2, SERVO_1_CENTER, SERVO_2_CENTER,
                   SERVO_3_CENTER);
    LG_spin_to_deg(&leg_group_3, SERVO_1_CENTER, SERVO_2_CENTER,
                   SERVO_3_CENTER);
    LG_spin_to_deg(&leg_group_4, SERVO_1_CENTER, SERVO_2_CENTER,
                   SERVO_3_CENTER);
    LG_spin_to_deg(&leg_group_5, SERVO_1_CENTER, SERVO_2_CENTER,
                   SERVO_3_CENTER);
    LG_spin_to_deg(&leg_group_6, SERVO_1_CENTER, SERVO_2_CENTER,
                   SERVO_3_CENTER);

    while (1) {
        // 等待开始循环开始
        if (!robot_control.start_loop) {
            // 舵机运动到初始位置
            LG_spin_to_deg(&leg_group_1, SERVO_1_CENTER + deg,
                           SERVO_2_CENTER - deg, SERVO_3_CENTER - deg);
            LG_spin_to_deg(&leg_group_2, SERVO_1_CENTER - deg,
                           SERVO_2_CENTER + deg, SERVO_3_CENTER + deg);
            LG_spin_to_deg(&leg_group_3, SERVO_1_CENTER, SERVO_2_CENTER,
                           SERVO_3_CENTER);
            LG_spin_to_deg(&leg_group_4, SERVO_1_CENTER, SERVO_2_CENTER,
                           SERVO_3_CENTER);
            LG_spin_to_deg(&leg_group_5, SERVO_1_CENTER, SERVO_2_CENTER,
                           SERVO_3_CENTER);
            LG_spin_to_deg(&leg_group_6, SERVO_1_CENTER, SERVO_2_CENTER,
                           SERVO_3_CENTER);

            // 舵机转到初始位置
            Servo_spin_to_deg(&test_servo, 135);

            // 气泵设置
            PumpGroup_set_all(&pump_group_all, robot_control.pump_all_on);

            // 胶水推杆设置
            glue_pusher(robot_control.glue);

            osDelay(20);
            continue;
        }

        // 计算当前时间
        uint32_t tick_count = xTaskGetTickCount();  // 获取系统时间，单位为Tick
        uint32_t milliseconds = tick_count * portTICK_PERIOD_MS;  // 转换为毫秒

        // 目标位置计算，结果保存在goal_pos中
        get_pos_by_path(center_x, center_y, center_z, 45.0f, milliseconds,
                        4000.0f, goal_pos);

        // 逆运动结算，结果存放在servo_rad中
        ik_status = Leg_inverse_kinematics(
            goal_pos[0], goal_pos[1], goal_pos[2], leg_1.leg_length,
            leg_2.leg_length, leg_3.leg_length, servo_rad);

        // 现在已经获得了各个关节舵机应转到的角度，存放在servo_rad中
        // servo_rad[0]->舵机1的角度
        // servo_rad[1]->舵机2的角度
        // servo_rad[2]->舵机3的角度

        // 这里可以添加代码来控制舵机转动到对应的角度↓
        // ..........

        if (robot_control.foot_style == TRIPEDAL) {
            walk_forward_tripedal();  // 三足前进
        } else if (robot_control.foot_style == FIVE_LEG) {
            walk_forward_five_leg();  // 五足前进
        }

        osDelay(WAIT_TIME);
    }
}

//-------------本地函数实现------------------
/**
 * @brief 通过路径计算目标位置
 * @param x 圆心位置x
 * @param y 圆心位置y
 * @param z 圆心位置z
 * @param radinus 圆半径
 * @param current_time 当前时间
 * @param loop_time 一次循环的时间
 * @param output 输出数组 [0:3]->x, y, z
 *
 */
static void get_pos_by_path(float x, float y, float z, float radinus,
                            float current_time, float loop_time,
                            float *output) {
    // 计算当前时间
    float current_time_in_loop = fmodf(current_time, loop_time);

    // 计算各段轨迹所需时间
    float circle_1_time = 0.45f * loop_time;
    float line_time = 0.1f * loop_time;
    float circle_2_time = 0.45f * loop_time;

    // 目标位置
    float goal_x_pos = x;  // 运动轨迹在圆心所在的YZ平面上
    float goal_y_pos = 0;
    float goal_z_pos = 0;

    // 一号轨迹（四分之一圆）
    if (current_time_in_loop < circle_1_time) {
        float angle = 0.5 * PI * (current_time_in_loop / circle_1_time);
        goal_y_pos = y - radinus * arm_sin_f32(angle);
        goal_z_pos = z - radinus * arm_cos_f32(angle);
    }

    // 二号轨迹（直线）
    else if (current_time_in_loop < circle_1_time + line_time) {
        float t = current_time_in_loop - circle_1_time;
        goal_y_pos = y - radinus + 2 * radinus / line_time * t;
        goal_z_pos = z;
    }

    // 三号轨迹（四分之一圆）
    else {
        float t = current_time_in_loop - circle_1_time - line_time;

        float angle = 0.5 * PI * (t / circle_2_time);
        goal_y_pos = y - radinus * arm_cos_f32(angle);
        goal_z_pos = z - radinus * arm_sin_f32(angle);
    }

    // 输出结果
    output[0] = goal_x_pos;
    output[1] = goal_y_pos;
    output[2] = goal_z_pos;
}

// ------------------ 运动函数------------------
// 三足步态
static void walk_forward_tripedal(void) {
    float pitch_J2 = robot_control.FL_pitch_change_J2;
    float pitch_J3 = robot_control.FL_pitch_change_J3;
    glue_pusher(1);

    // A组腿抬腿并向前移动
    LG_spin_to_deg(&leg_group_1, SERVO_1_CENTER - robot_control.yaw_change,
                   SERVO_2_CENTER - robot_control.TR_pitch_change_J2,
                   SERVO_3_CENTER + robot_control.TR_pitch_change_J3);
    LG_spin_to_deg(&leg_group_3, SERVO_1_CENTER + robot_control.yaw_change,
                   SERVO_2_CENTER + robot_control.TR_pitch_change_J2,
                   SERVO_3_CENTER - robot_control.TR_pitch_change_J3);
    LG_spin_to_deg(&leg_group_5, SERVO_1_CENTER - robot_control.yaw_change,
                   SERVO_2_CENTER - robot_control.TR_pitch_change_J2,
                   SERVO_3_CENTER + robot_control.TR_pitch_change_J3);
    osDelay(3000);
    PumpGroup_set_all(&pump_group_a, 1);  // A组气泵打开
    PumpGroup_set_all(&pump_group_b, 0);  // B组气泵关闭

    // A组放下腿
    LG_spin_to_deg(&leg_group_1, SERVO_1_CENTER - robot_control.yaw_change,
                   SERVO_2_CENTER, SERVO_3_CENTER);
    LG_spin_to_deg(&leg_group_3, SERVO_1_CENTER + robot_control.yaw_change,
                   SERVO_2_CENTER, SERVO_3_CENTER);
    LG_spin_to_deg(&leg_group_5, SERVO_1_CENTER - robot_control.yaw_change,
                   SERVO_2_CENTER, SERVO_3_CENTER);
    osDelay(3000);

    // A组后摆
    LG_spin_to_deg(&leg_group_1, SERVO_1_CENTER + robot_control.yaw_change,
                   SERVO_2_CENTER, SERVO_3_CENTER);
    LG_spin_to_deg(&leg_group_3, SERVO_1_CENTER - robot_control.yaw_change,
                   SERVO_2_CENTER, SERVO_3_CENTER);
    LG_spin_to_deg(&leg_group_5, SERVO_1_CENTER + robot_control.yaw_change,
                   SERVO_2_CENTER, SERVO_3_CENTER);

    // B组腿抬腿并向前移动
    LG_spin_to_deg(&leg_group_2, SERVO_1_CENTER + robot_control.yaw_change,
                   SERVO_2_CENTER + robot_control.TR_pitch_change_J2,
                   SERVO_3_CENTER - robot_control.TR_pitch_change_J3);
    LG_spin_to_deg(&leg_group_4, SERVO_1_CENTER + robot_control.yaw_change,
                   SERVO_2_CENTER + robot_control.TR_pitch_change_J2,
                   SERVO_3_CENTER - robot_control.TR_pitch_change_J3);
    LG_spin_to_deg(&leg_group_6, SERVO_1_CENTER - robot_control.yaw_change,
                   SERVO_2_CENTER - robot_control.TR_pitch_change_J2,
                   SERVO_3_CENTER + robot_control.TR_pitch_change_J3);
    osDelay(3000);
    PumpGroup_set_all(&pump_group_a, 0);  // A组气泵关闭
    PumpGroup_set_all(&pump_group_b, 1);  // B组气泵打开

    // B组放下腿
    LG_spin_to_deg(&leg_group_2, SERVO_1_CENTER + robot_control.yaw_change,
                   SERVO_2_CENTER, SERVO_3_CENTER);
    LG_spin_to_deg(&leg_group_4, SERVO_1_CENTER + robot_control.yaw_change,
                   SERVO_2_CENTER, SERVO_3_CENTER);
    LG_spin_to_deg(&leg_group_6, SERVO_1_CENTER - robot_control.yaw_change,
                   SERVO_2_CENTER, SERVO_3_CENTER);
    osDelay(3000);

    // B组后摆
    LG_spin_to_deg(&leg_group_2, SERVO_1_CENTER - robot_control.yaw_change,
                   SERVO_2_CENTER, SERVO_3_CENTER);
    LG_spin_to_deg(&leg_group_4, SERVO_1_CENTER - robot_control.yaw_change,
                   SERVO_2_CENTER, SERVO_3_CENTER);
    LG_spin_to_deg(&leg_group_6, SERVO_1_CENTER + robot_control.yaw_change,
                   SERVO_2_CENTER, SERVO_3_CENTER);
}

// 五足步态
static void walk_forward_five_leg(void) {
    float yaw = robot_control.yaw_change;
    float yaw_fix_in = robot_control.yaw_fix;
    float pitch_J2 = robot_control.FL_pitch_change_J2;
    float pitch_J3 = robot_control.FL_pitch_change_J3;

    // 抬腿并向前移动，放腿
    for (uint8_t i = 0; i < 6; i++) {
        // 为了凑够打胶时间，在最后一条腿迈出时，开始打胶
        if (i == 5) {
            glue_pusher(1);  // 打胶
        }

        // 关闭对应的气泵
        PumpGroup_set(&pump_group_all, i, 0);

        // i>2 时，说明此时为右腿组，符号为左边相反
        if (i > 2) {
            yaw = -robot_control.yaw_change;
            pitch_J2 = -robot_control.FL_pitch_change_J2;
            pitch_J3 = -robot_control.FL_pitch_change_J3;
        } else {
            yaw = robot_control.yaw_change;
            pitch_J2 = robot_control.FL_pitch_change_J2;
            pitch_J3 = robot_control.FL_pitch_change_J3;
        }
        // 抬腿并向前移动;
        LG_spin_to_deg(leg_group_array[i], SERVO_1_CENTER - yaw,
                       SERVO_2_CENTER - pitch_J2, SERVO_3_CENTER + pitch_J3);

        osDelay(3000);

        // 开启对应气泵，开始吸气
        PumpGroup_set(&pump_group_all, i, 1);

        // 放下腿
        LG_spin_to_deg(leg_group_array[i], SERVO_1_CENTER - yaw, SERVO_2_CENTER,
                       SERVO_3_CENTER);
        osDelay(3000);
    }

    // 后摆，开始打胶
    for (uint8_t i = 0; i < 6; i++) {
        // i>2 时，说明此时为右腿组，符号为左边相反
        if (i > 2) {
            yaw = -robot_control.yaw_change;
            yaw_fix_in = -robot_control.yaw_fix;
            pitch_J2 = -robot_control.FL_pitch_change_J2;
        } else {
            yaw = robot_control.yaw_change;
            pitch_J2 = robot_control.FL_pitch_change_J2;
        }

        LG_spin_to_deg(leg_group_array[i], SERVO_1_CENTER + yaw - yaw_fix_in,
                       SERVO_2_CENTER, SERVO_3_CENTER);
    }
    osDelay(3000);
    glue_pusher(0);  // 停止打胶
}

static void glue_pusher(_Bool pusher_on) {
    if (pusher_on) {
        PWM_set_full(&glue_pusher_ctl);
    } else {
        PWM_set_zero(&glue_pusher_ctl);
    }
}

RobotControl *get_rebot_control_pointer(void) { return &robot_control; }
